﻿using System;
using System.Collections.Generic;
using System.Diagnostics.Contracts;
using System.Linq;
using System.Text;
using EuroManager.Domain.Simulator.Engine.Movements;
using EuroManager.Domain.Simulator.Helpers;

namespace EuroManager.Domain.Simulator.Engine
{
    public class Steering : ISteering
    {
        private static readonly float maxTurnRate = 2 * (float)Maths.Pi;
        
        private Vector2D location;
        private float acceleration;
        private float deceleration;
        private float maxSpeed;
        private Vector2D velocity = Vector2D.Zero;
        private Vector2D heading = Vector2D.UnitX;
        private Vector2D desiredHeading = Vector2D.UnitX;
        private IMovement movement = null;

        public Steering(Vector2D location, float acceleration, float deceleration, float maxSpeed)
        {
            Contract.Requires(acceleration.IsRational() && acceleration >= 0);
            Contract.Requires(deceleration.IsRational() && deceleration >= 0);
            Contract.Requires(maxSpeed.IsRational() && maxSpeed >= 0);

            this.location = location;
            this.acceleration = acceleration;
            this.deceleration = deceleration;
            this.maxSpeed = maxSpeed;

            Stop();
        }

        public Vector2D Location
        {
            get { return location; }
        }

        public Vector2D Velocity
        {
            get { return velocity; }
        }

        public Vector2D Heading
        {
            get { return heading; }
        }

        public float Speed
        {
            get { return velocity.Length; }
        }

        public IMovement Movement
        {
            get { return movement; }
        }

        public void Update(float elapsedTime)
        {
            movement.Update(elapsedTime);
            location = movement.Location;
            velocity = movement.Velocity;

            UpdateHeading(elapsedTime);

            if (movement.IsFinished)
            {
                Stop();
            }
        }

        public void Turn(Vector2D target)
        {
            if (target != location)
            {
                desiredHeading = (target - location).Normalized();
            }
        }

        public void Move(IMovement movement)
        {
            this.movement = movement;
        }

        public void Stop()
        {
            Move(new StillMovement(location, velocity, deceleration));
        }

        public void Halt()
        {
            Move(new StillMovement(location, Vector2D.Zero, deceleration));
        }

        public void Hit(Vector2D startVelocity)
        {
            Move(new HitMovement(location, startVelocity));
        }

        public void Meet(Vector2D destination, float tolerance, float time)
        {
            Move(new ArriveMovement(location, velocity, destination, acceleration, deceleration, maxSpeed, tolerance, time));
        }

        public void Arrive(Vector2D destination, float tolerance)
        {
            Meet(destination, tolerance, 0);
        }

        private void UpdateHeading(float elapsedTime)
        {
            if (!velocity.IsZero)
            {
                desiredHeading = Velocity.Normalized();
            }

            if (desiredHeading != heading)
            {
                float dot = Vector2D.Dot(heading, desiredHeading);
                float angle = Maths.Acos(dot);

                if (dot < 0)
                {
                    angle = -angle;
                }

                if (Maths.Abs(angle) <= maxTurnRate * elapsedTime)
                {
                    heading = desiredHeading;
                }
                else
                {
                    float newAngle = maxTurnRate * elapsedTime;

                    if (Vector2D.Dot(heading.Perpendicular(), desiredHeading) < 0)
                    {
                        newAngle = -newAngle;
                    }

                    heading = heading.Rotated(newAngle).Normalized();
                }
            }
        }
    }
}
